﻿using System;
using System.Collections.Generic;
using System.Text;
using LocalizationTest.Tools;
using System.Drawing;

namespace LocalizationTest
{
  class MBLRefinementSelfLocator : SelfLocator
  {
    ActionAndObservationHistory actionAndObservationHistory;
    public override WorldState Execute(Experiment experiment)
    {
      actionAndObservationHistory = experiment.actionAndObservationHistory;
      if (experiment.actionAndObservationHistory.history.Count == 0) return null;
      List<ActionAndObservationHistory.RelativePlaceWithMeasurement> listOfOldPlaces = experiment.actionAndObservationHistory.GetSelectedRelativePlaces(experiment.simulatedWorldState);
      if (listOfOldPlaces.Count == 0) return null;

      int numberOfRefinements = 1;

      if (DebugKeyTool.IsActive("MBL: multiple refinements")) numberOfRefinements = 4;

      Vector2 bottomLeftCorner = new Vector2(Globals.stateSpaceMinimumX, Globals.stateSpaceMinimumY);
      Vector2 topRightCorner = new Vector2(Globals.stateSpaceMaximumX, Globals.stateSpaceMaximumY);
      int rasterWidth = 32;// Globals.rasterWidth;

      if (DebugKeyTool.IsActive("MBL: fine grid")) rasterWidth = 4;

      double refinementFactor = 0.25;

      double minimalValue;
      Vector2 positionOfMinimalValue = new Vector2();

      for (int i = 0; i < numberOfRefinements; i++)
      {
        TimeTool.Start("MBL main loop" + i);
        double[,] array;
        List<Function2D> function;
        double maximalValue;
        Vector2 positionOfMaximalValue;

        FindMinimum(listOfOldPlaces, bottomLeftCorner, topRightCorner, rasterWidth,
          out array, out function,
          out minimalValue, out positionOfMinimalValue,
          out maximalValue, out positionOfMaximalValue,
          experiment
          );
        TimeTool.Stop("MBL main loop" + i);

        int oldRasterWidth = rasterWidth;
        rasterWidth = (int)(rasterWidth * refinementFactor);
        Vector2 newRange = (topRightCorner - bottomLeftCorner);
        newRange *= refinementFactor;
        newRange.x -= newRange.x % oldRasterWidth;
        newRange.y -= newRange.y % oldRasterWidth;
        bottomLeftCorner = positionOfMinimalValue - newRange / 2;
        topRightCorner = positionOfMinimalValue + newRange / 2;

        DrawFunction("world:MBL:likelihood function ll", function, minimalValue, maximalValue, oldRasterWidth, rasterWidth != 0 && i != numberOfRefinements - 1 && numberOfRefinements != 1, bottomLeftCorner, topRightCorner);
        DebugDrawingTool.CreateRectangle("world:MBL:position", bottomLeftCorner, topRightCorner, 2, System.Drawing.Color.Green);
        if (DebugKeyTool.IsActive("ExportToExcel"))
        {
          if (i == 1)
            ExcelViewer.ViewWithExcel(array);
        }
        if (rasterWidth == 0) break;
      }
      CalculateContribution(experiment, listOfOldPlaces, positionOfMinimalValue);
      return new WorldState2D(positionOfMinimalValue.x, positionOfMinimalValue.y);
    }

    private void CalculateContribution(Experiment setup, List<ActionAndObservationHistory.RelativePlaceWithMeasurement> listOfOldPlaces, Vector2 positionOfMinimalValue)
    {
      if (DebugKeyTool.IsActive("calculate laplace"))
      {
        //        CalculateLaplaceForAll(bottomLeftCorner, topRightCorner);
      }
      DebugDrawingTool.CreateCircle("world:MBL:position", positionOfMinimalValue, 5, 2, System.Drawing.Color.Green);

      double laplaceValue = 1000 * MBLTools.CalculateLaplace(listOfOldPlaces, positionOfMinimalValue.x, positionOfMinimalValue.y, setup);
      DebugDrawingTool.CreateText("world:laplace", laplaceValue.ToString("000.000"), positionOfMinimalValue + new Vector2(50, 50), 15, Color.Red);

      int spacing = 15;

      Vector2[,] gradient = new Vector2[3, 3];

      for (int xOffset = -1; xOffset <= 1; xOffset += 1)
      {
        for (int yOffset = -1; yOffset <= 1; yOffset += 1)
        {
          Vector2 offset = new Vector2(xOffset * spacing, yOffset * spacing);
          gradient[xOffset + 1, yOffset + 1] = MBLTools.CalculateGradient(listOfOldPlaces, positionOfMinimalValue.x + offset.x, positionOfMinimalValue.y + offset.y, setup);
          DebugDrawingTool.CreateArrow("world:gradients", positionOfMinimalValue + offset, positionOfMinimalValue + gradient[xOffset + 1, yOffset + 1] * 10000 + offset, 2, Color.Orange);
        }
      }
      double quotient = (gradient[0, 1].Abs() + gradient[2, 1].Abs()) / (gradient[1, 0].Abs() + gradient[1, 2].Abs());
      DebugDrawingTool.CreateText("world:laplace", quotient.ToString("000.000"), positionOfMinimalValue + new Vector2(50, 100), 15, Color.Yellow);

      //OLD CONTRIBUTION STUFF
      /*

      double directionOfSteepestDescent = CalculateDirectionOfSteepestDescentNearExtremum(listOfOldPlaces, positionOfMinimalValue, spacing, Color.Green, setup);
      double smallestContribution = Math.PI;
      double directionWithSmallestContribution = 0;
      int counter = 0;
      int indexOfSmallestContribution = 0;

      this.actionAndObservationHistory.contribution.Clear();
      foreach (ActionAndObservationHistory.RelativePlacesInfo entry in listOfOldPlaces)
      {
        List<ActionAndObservationHistory.RelativePlacesInfo> smallList = new List<ActionAndObservationHistory.RelativePlacesInfo>();
        smallList.Add(entry);
        double currentDirection = CalculateDirectionOfSteepestDescentNearExtremum(smallList, positionOfMinimalValue, spacing, Color.SkyBlue, setup);
        double contribution = Math.Abs( MathTools.NormalizeSmall(currentDirection - directionOfSteepestDescent));
        this.actionAndObservationHistory.contribution.Add(contribution);
        if (counter != 0 && contribution < smallestContribution)
        {
          smallestContribution = contribution;
          directionWithSmallestContribution = currentDirection;
          indexOfSmallestContribution = entry.indexInHistory;
        }
        counter++;
      }
      if (DebugKeyTool.IsActive("Remove history entry with lowest contribution"))
      {
        if (actionAndObservationHistory.history[actionAndObservationHistory.history.Count - 1] is Measurement)
        {
          int measurementCounter = 0;
          int actionCounter = 0;
          foreach (object entry in this.actionAndObservationHistory.history)
          {
            if (entry is Measurement)
            {
              if (actionCounter == indexOfSmallestContribution)
                actionAndObservationHistory.markedAsDeleted[actionCounter] = true;
              measurementCounter++;
            }
            actionCounter++;
          }
        }
      }
      Vector2 directionOfSmallestContribution = new Vector2(Math.Cos(directionWithSmallestContribution), Math.Sin(directionWithSmallestContribution));
      DebugDrawingTool.CreateLine("world:circle 2", positionOfMinimalValue - directionOfSmallestContribution * 100, positionOfMinimalValue + directionOfSmallestContribution * 100, 4, Color.Red);
      */ 
    }

    private void FindMinimum(
      List<ActionAndObservationHistory.RelativePlaceWithMeasurement> listOfOldPlaces,
      Vector2 bottomLeftCorner,
      Vector2 topRightCorner,
      int rasterWidth,
      out double[,] array,
      out List<Function2D> function,
      out double minimalValue,
      out Vector2 positionOfMinimalValue,
      out double maximalValue,
      out Vector2 positionOfMaximalValue,
      Experiment setup)
    {
      int nuberOfRows = (int)(topRightCorner.x - bottomLeftCorner.x) / rasterWidth + 2;
      int nuberOfColums = (int)(topRightCorner.y - bottomLeftCorner.y) / rasterWidth + 2;
      array = new double[nuberOfRows, nuberOfColums];

      function = new List<Function2D>();
      maximalValue = 0;
      minimalValue = Double.MaxValue;

      positionOfMinimalValue = new Vector2();
      positionOfMaximalValue = new Vector2();

      int xCounter = 0;
      int yCounter = 0;
      for (int x = (int)bottomLeftCorner.x; x <= (int)topRightCorner.x; x += rasterWidth)
      {
        yCounter = 0;
        for (int y = (int)bottomLeftCorner.y; y <= (int)topRightCorner.y; y += rasterWidth)
        {
          double value = MBLTools.CalculateFunction(listOfOldPlaces, x, y, setup);

          function.Add(new Function2D(x, y, value));
          array[xCounter, yCounter] = value;
          if (value > maximalValue)
          {
            maximalValue = value;
            positionOfMaximalValue.x = x;
            positionOfMaximalValue.y = y;
          }
          if (value < minimalValue)
          {
            minimalValue = value;
            positionOfMinimalValue.x = x;
            positionOfMinimalValue.y = y;
          }
          yCounter++;
        }//for y..
        xCounter++;
      }//for x..
    }

    private static void DrawFunction(string drawingID,
      List<Function2D> function, double minimalValue, double maximalValue,
      int rasterWidth, bool excludeRange, Vector2 bottomLeftCornerExlude, Vector2 topRightCornerExclude)
    {
      if (maximalValue - minimalValue == 0) return;
      foreach (Function2D entry in function)
      {
        if (//maximalValue > 0.00001 && 
          (!excludeRange ||
          entry.x < bottomLeftCornerExlude.x || entry.x > topRightCornerExclude.x ||
          entry.y < bottomLeftCornerExlude.y || entry.y > topRightCornerExclude.y
          ))
        {
          int intensity = (int)(Math.Pow((1 - (entry.value - minimalValue) / (maximalValue - minimalValue)), 100) * 250.0);
          if (DebugKeyTool.IsActive("options:transparent function"))
          {
            DebugDrawingTool.CreateLine(drawingID,
            new Vector2(entry.x - rasterWidth / 2.0, entry.y), new Vector2(entry.x + rasterWidth / 2.0 + 1, entry.y),
            rasterWidth + 1, System.Drawing.Color.FromArgb(150, intensity, intensity, intensity));
          }
          else
          {
            DebugDrawingTool.CreateLine(drawingID,
            new Vector2(entry.x - rasterWidth / 2.0, entry.y), new Vector2(entry.x + rasterWidth / 2.0 + 1, entry.y),
            rasterWidth + 1, System.Drawing.Color.FromArgb(intensity, intensity, intensity));
          }
        }
      }
    }

    private double CalculateDirectionOfSteepestDescentNearExtremum(List<ActionAndObservationHistory.RelativePlaceWithMeasurement> listOfOldPlaces, Vector2 positionOfMinimalValue, int spacing, Color colorOfDrawings, Experiment setup)
    {
      int numberOfTests = 10;
      Vector2[] gradientList = new Vector2[numberOfTests];
      double max = 0;
      int indexOfMax = 0;
      for (int i = 0; i < numberOfTests; i++)
      {
        double angle = Math.PI * 2.0 * i / (double)numberOfTests;
        Vector2 offset = new Vector2(spacing * Math.Cos(angle), spacing * Math.Sin(angle));
        gradientList[i] = MBLTools.CalculateGradient(listOfOldPlaces, positionOfMinimalValue.x + offset.x, positionOfMinimalValue.y + offset.y, setup);
        if (gradientList[i].Abs() > max)
        {
          max = gradientList[i].Abs();
          indexOfMax = i;
        }
        DebugDrawingTool.CreateArrow("world:circle", positionOfMinimalValue + offset, positionOfMinimalValue + gradientList[i] * 10000 + offset, 1, colorOfDrawings);
      }
      gradientList[indexOfMax].Normalize();
      DebugDrawingTool.CreateLine("world:circle 2", positionOfMinimalValue - gradientList[indexOfMax] * 200, positionOfMinimalValue + gradientList[indexOfMax] * 200, 4, colorOfDrawings);
      double directionOfSteepestDescent = gradientList[indexOfMax].GetAngle();
      return directionOfSteepestDescent;
    }

  }
}
